﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Windows.Forms;
using QuadRemote.Core;

namespace QuadRemote.Panels
{

    class GyroscopePanel : Graph
    {
        private Queue<float> qx;
        private Queue<float> qy;
        private Queue<float> qz;
        private Queue<float> qxf;
        private Queue<float> qyf;
        private Queue<float> qzf;
        private long lastUpdate;
        private int waitSamples = 16;

        private GraphData x;
        private GraphData y;
        private GraphData z;
        private GraphData xf;
        private GraphData yf;
        private GraphData zf;


        public GyroscopePanel()
            : base("Gyroscope", 30, 2, Color.Magenta)
        {

            qx = new Queue<float>();
            qy = new Queue<float>();
            qz = new Queue<float>();
            qxf = new Queue<float>();
            qyf = new Queue<float>();
            qzf = new Queue<float>();

            _gain = 0.995F;

            
            x = new GraphData();
            x.display = GraphDisplay.REALTIME;
            x.name = "X";
            x.scale = 1;
            x.type = GraphType.NORMAL;
            x.unit = GraphUnit.FLOAT;
            x.pen = new Pen(Color.FromArgb(255, 0, 0), 2);

            y = new GraphData();
            y.display = GraphDisplay.REALTIME;
            y.name = "Y";
            y.scale = 1;
            y.type = GraphType.NORMAL;
            y.unit = GraphUnit.FLOAT;
            y.pen = new Pen(Color.FromArgb(0, 255, 0), 2);

            z = new GraphData();
            z.display = GraphDisplay.REALTIME;
            z.name = "Z";
            z.scale = 1;
            z.type = GraphType.NORMAL;
            z.unit = GraphUnit.FLOAT;
            z.pen = new Pen(Color.FromArgb(0, 0, 255), 2);

            xf = new GraphData();
            xf.display = GraphDisplay.REALTIME;
            xf.name = "X";
            xf.scale = 1;
            xf.type = GraphType.NORMAL;
            xf.unit = GraphUnit.FLOAT;
            xf.pen = new Pen(Color.FromArgb(128, 0, 0), 2);

            yf = new GraphData();
            yf.display = GraphDisplay.REALTIME;
            yf.name = "Y";
            yf.scale = 1;
            yf.type = GraphType.NORMAL;
            yf.unit = GraphUnit.FLOAT;
            yf.pen = new Pen(Color.FromArgb(0, 128, 0), 2);

            zf = new GraphData();
            zf.display = GraphDisplay.REALTIME;
            zf.name = "Z";
            zf.scale = 1;
            zf.type = GraphType.NORMAL;
            zf.unit = GraphUnit.FLOAT;
            zf.pen = new Pen(Color.FromArgb(0, 0, 128), 2);

            graphs.Add(x);
            graphs.Add(y);
            graphs.Add(z);

            graphs.Add(xf);
            graphs.Add(yf);
            graphs.Add(zf);

            init();
        }
        public void queueRaw(float a, float b, float c)
        {
            lock (qx)
            {
                qx.Enqueue(a);
                qy.Enqueue(b);
                qz.Enqueue(c);
            }
        }

        public void queueFiltered(float a, float b, float c)
        {
            qxf.Enqueue(a);
            qyf.Enqueue(b);
            qzf.Enqueue(c);
        }

        private float dynamicF = 20;

        public override void PaintPanel(System.Drawing.Graphics g)
        {
            int[] sizes = { qx.Count, qxf.Count };

            int max = sizes[0];
            if (sizes[1] > max) max = sizes[1];

            Queue<float>[,] queues = { { qx, qy, qz }, { qxf, qyf, qzf } };
            GraphData[,] graphs = { { x, y, z }, { xf, yf, zf } };

            if (max >= waitSamples)
            {
                float val1, val2, val3;

                for (int o = 0; o < sizes.Length; o++)
                {
                    int bufferBegin = bufferOffset;

                    if (sizes[o] == 0)
                    {
                        val1 = graphs[o, 0].data[bufferBegin];
                        val2 = graphs[o, 1].data[bufferBegin];
                        val3 = graphs[o, 2].data[bufferBegin];

                        for (int i = 0; i < max; i++)
                        {
                            bufferBegin++;
                            if (bufferBegin == bufferSize) bufferBegin = 0;
                            graphs[o, 0].data[bufferBegin] = val1;
                            graphs[o, 1].data[bufferBegin] = val2;
                            graphs[o, 2].data[bufferBegin] = val3;
                        }
                        continue;
                    }

                    float t = 0;

                    lock (queues[o,0])
                    {
                        val1 = queues[o,0].Dequeue();
                        val2 = queues[o,1].Dequeue();
                        val3 = queues[o,2].Dequeue();

                        for (int i = 0; i < max; i++)
                        {
                            bufferBegin++;
                            if (bufferBegin == bufferSize) bufferBegin = 0;
                            graphs[o,0].data[bufferBegin] = val1;
                            graphs[o,1].data[bufferBegin] = val2;
                            graphs[o,2].data[bufferBegin] = val3;

                            t += max / sizes[o];
                            if (t >= 1 && queues[o,0].Count > 0)
                            {
                                val1 = queues[o,0].Dequeue();
                                val2 = queues[o,1].Dequeue();
                                val3 = queues[o,2].Dequeue();
                                t -= 1;
                            }
                        }
                    }
                }

                bufferOffset += max;
                if (bufferOffset >= bufferSize) bufferOffset -= bufferSize;


                long tickDiff = ((DateTime.Now.Ticks - lastUpdate) / TimeSpan.TicksPerMillisecond);
                if (tickDiff != 0)
                {
                    float f = 1000.0F / tickDiff * waitSamples;

                    dynamicF = (int)(dynamicF * 0.98F + f * 0.02F);
                    _freq = (int)Math.Round(dynamicF);
                }

                lastUpdate = DateTime.Now.Ticks;
            }

            base.PaintPanel(g);
        }
    }
}
